#include "path_planning/topological_map.h"

AdjNodeList::~AdjNodeList(void){
	AdjNode *node = first_, *node_to_delete;
	while(node){
		node_to_delete = node;
		node = node->next;
		delete node_to_delete;
	}
	node = NULL;
	node_to_delete = NULL;
	first_ = NULL;
	last_ = NULL;
}

void AdjNodeList::Add(int adjacent_id, double dist){
	AdjNode *new_node = new AdjNode();
	new_node->dist = dist;
	new_node->adjacent_id = adjacent_id;
	new_node->next = NULL;
	if(!first_){
		first_ = new_node;
		last_ = new_node;
	}else{
		last_->next = new_node;
		last_ = new_node;
	}
}

AdjNode* AdjNodeList::Remove(int adjacent_id){
	if(!first_)
		return NULL;
	if(first_->adjacent_id == adjacent_id){
		AdjNode *aux = first_;
		first_ = first_->next;
		return aux;
	}
	AdjNode *adj_node = first_->next;
	AdjNode *last_adj_node = first_;
	while(adj_node){
		if(adj_node->adjacent_id == adjacent_id){
			last_adj_node->next = adj_node->next;
			if(last_ == adj_node)
				last_ = last_adj_node;
			return adj_node;
		}
		last_adj_node = adj_node;
		adj_node = adj_node->next;
	}
	return NULL;
}

AdjNode* AdjNodeList::GetFirst(){ return first_; }

AdjNode* AdjNodeList::GetLast(){ return last_; }


AdjacencyList::AdjacencyList(int size) : size_(size){
	list_of_vertex_ = new Vertex*[size_];
	max_verts_ = INT_MAX;
	edges_ = 0;

	if(!list_of_vertex_){
		cerr << "Out of space in AdjacencyList" << endl;
		exit(-1);
	}

	for(int i = 0; i < size_; i++){
		Vertex *new_vertex = new Vertex();
		new_vertex->id=i;
		new_vertex->f = INT_MAX;
		new_vertex->g = 0;
		new_vertex->h = INT_MAX;
		new_vertex->x_coordinate = kUnknowValue;
		new_vertex->y_coordinate = kUnknowValue;
		new_vertex->list_of_adjacents = new AdjNodeList();

		list_of_vertex_[i] = new_vertex;
	}
}

AdjacencyList::AdjacencyList(void){ }

AdjacencyList::~AdjacencyList(){
	for(int i = 0; i < size_; i++){
		delete list_of_vertex_[i]->list_of_adjacents;
		list_of_vertex_[i]->list_of_adjacents = NULL;
		delete list_of_vertex_[i];
		list_of_vertex_[i] = NULL;
	}
	delete[] list_of_vertex_;
	list_of_vertex_ = NULL;

}

bool AdjacencyList::AddEdge(int verta, int vertb){
	if(verta > size_ || vertb > size_){
		cerr << "Number of vertices exceeds size of " << size_ << " in AdjacencyList::AddEdge !" << endl;
		cerr << "Exiting...." << endl;
		return false;
	}

	edges_++;

	double edge = sqrt(pow(list_of_vertex_[verta]->x_coordinate - list_of_vertex_[vertb]->x_coordinate, 2) +
				  pow(list_of_vertex_[verta]->y_coordinate - list_of_vertex_[vertb]->y_coordinate, 2));

	//set vertb as adjacent node of verta.
	list_of_vertex_[verta]->list_of_adjacents->Add(vertb, edge);

	//set verta as adjacent node of vertb.
	list_of_vertex_[vertb]->list_of_adjacents->Add(verta, edge);

	return true;
}

bool AdjacencyList::RemoveEdge(int verta, int vertb){
	if(verta > size_ || vertb > size_){
		cerr << "Number of vertices exceeds size of " << size_ << " in AdjacencyList::AddEdge !" << endl;
		return false;
	}

	AdjNode* adj_a = list_of_vertex_[verta]->list_of_adjacents->Remove(vertb);
	AdjNode* adj_b = list_of_vertex_[vertb]->list_of_adjacents->Remove(verta);

	bool res = false;
	if(adj_a != NULL && adj_b != NULL){
		res = true;
		edges_--;
	}

	if(adj_a != NULL){
		delete adj_a;
		adj_a = NULL;
	}

	if(adj_b != NULL){
		delete adj_b;
		adj_b = NULL;
	}

	return res;
}

Vertex* AdjacencyList::GetVertexById(int vertex_id){
	if(vertex_id > size_){
		cerr << "Vertex Id exceeds size of " << size_ << " in AdjacencyList::GetVertexById !" << endl;
		exit(-1);
	}

	return list_of_vertex_[vertex_id];
}

int AdjacencyList::edges() const { return edges_; }

int AdjacencyList::size() const { return size_; }

PlannerMap::~PlannerMap(){ delete topological_map_; }

bool PlannerMap::SetGraphDescriptor(string graph_descriptor_path_file){
	cout << "CALL PlannerMap::SetGraphDescriptor -" << endl;

	graph_descriptor_path_file_ = graph_descriptor_path_file;

	const char* path = graph_descriptor_path_file.c_str();
	ifstream file;
	file.open(path, ios::in | ios::ate);
	if(!file.is_open()){
		cerr << "PlannerMap::SetGraphDescriptor: Invalid file (" << graph_descriptor_path_file << "). END" << endl;
		return false;
	}

	char* file_data;
	ifstream::pos_type size;
	size = file.tellg();
	file_data = new char [size];
	file.seekg (0, ios::beg);
	file.read (file_data, size);
	file.close();

	char* context;
	int number_of_vertices = atoi(strtok_r(file_data, " \n", &context));
	if(!number_of_vertices){
		cerr << "PlannerMap::SetGraphDescriptor: Error while trying to parse " << graph_descriptor_path_file <<
				" file. Invalid 'number_of_vertices' parameter. END" << endl;
		return false;
	}

	topological_map_ = new AdjacencyList(number_of_vertices);

	for(int i = 0; i < number_of_vertices; i++){
		int id = atoi(strtok_r(NULL, " \n", &context));
		double x_coord = atof(strtok_r(NULL, " \n", &context));
		double y_coord = atof(strtok_r(NULL, " \n", &context));
		Vertex* vertex = topological_map_->GetVertexById(id);
		vertex->x_coordinate = x_coord;
		vertex->y_coordinate = y_coord;
	}

	int number_of_edges = atoi(strtok_r(NULL, " \n", &context));
	if(!number_of_edges){
		cerr << "PlannerMap::SetGraphDescriptor: Error while trying to parse " << graph_descriptor_path_file <<
				" file. Invalid 'number_of_edges' parameter. END" << endl;
		return false;
	}

	for(int i = 0; i < number_of_edges; i++){
		int verta = atoi(strtok_r(NULL, " \n", &context));
		int vertb = atoi(strtok_r(NULL, " \n", &context));

		topological_map_->AddEdge(verta, vertb);
	}
	cout << "CALL PlannerMap::SetGraphDescriptor ok - END" << endl;
	return true;
}

bool PlannerMap::SetGridMapDescriptor(string grid_map_path_file){
	cout << "CALL PlannerMap::SetGridMapDescriptor -" << endl;

	const char* path = grid_map_path_file.c_str();
	ifstream file;
	file.open(path, ios::in | ios::ate);
	if(!file.is_open()){
		cerr << "PlannerMap::SetGridMapDescriptor: Invalid file (" << grid_map_path_file << "). END" << endl;
		return false;
	}

	char* file_data;
	ifstream::pos_type size;
	size = file.tellg();
	file_data = new char [size];
	file.seekg (0, ios::beg);
	file.read (file_data, size);
	file.close();

	char* context;
	grid_map_resolution_ = atof(strtok_r(file_data, " \n", &context));
	grid_map_width_ = atoi(strtok_r(NULL, " \n", &context));
	grid_map_height_ = atoi(strtok_r(NULL, " \n", &context));

	if(grid_map_matrix_)
			delete[] grid_map_matrix_;

	grid_map_matrix_ = new int[grid_map_width_*grid_map_height_];

	grid_map_matrix_ = new int[grid_map_width_*grid_map_height_];
	for(int i = 0; i < grid_map_height_; i++)
		for(int j = 0; j < grid_map_width_; j++)
			grid_map_matrix_[i*grid_map_width_ + j] = atoi(strtok_r(NULL, " \n", &context));

	cout << "CALL PlannerMap::SetGridMapDescriptor. ok - END" << endl;
	return true;
}

bool PlannerMap::RemoveEdge(int vertex_1_id, int vertex_2_id){
	cout << "CALL PlannerMap::RemoveEdge -" << endl;

	bool res = true;
	if(topological_map_->RemoveEdge(vertex_1_id, vertex_2_id)){
		cout << "PlannerMap::Edge removed" << endl;
	}else{
		cerr << "PlannerMap::RemoveEdge. Failure. There is no edge between " << vertex_1_id << "and " << vertex_2_id << endl;
		res = false;
	}

	cout << "CALL PlannerMap::RemoveEdge. ok - END" << endl;
	return res;
}

bool PlannerMap::AddEdge(int vertex_1_id, int vertex_2_id){
	cout << "CALL PlannerMap::AddEdge -" << endl;

	if(topological_map_->AddEdge(vertex_1_id, vertex_2_id)){
		cout << "CALL PlannerMap::AddEdge. Ok. END" << endl;
		return true;
	}

	cerr << "CALL PlannerMap::AddEdge. Failure. END" << endl;
	return false;
}

Vertex* PlannerMap::GetClosestVertex(double x_coordinate, double y_coordinate){
	cout << "CALL PlannerMap::GetClosestVertex -" << endl;

	PixelPoint* point = RealCoordToPixel(x_coordinate, y_coordinate, grid_map_resolution_ , grid_map_width_, grid_map_height_);

	Vertex* closest_vertex = topological_map_->GetVertexById(grid_map_matrix_[point->y*grid_map_width_ + point->x]);

	cout << "PlannerMap::GetClosestVertex ok - END." << endl;
	return closest_vertex;
}

void PlannerMap::GetClosestVertices(player_point_2d origin, player_point_2d destination,
	   	   	   	   	   Vertex* &closest_vertex_to_origin, Vertex* &closest_vertex_to_destination){
	   cout << "CALL PlannerMap::GetClosestVertices -" << endl;

	   if(topological_map_->size() == 0){
		   cerr << "PlannerMap::GetClosestVertices: Topological map not set (Maybe you didn't call PahPlanner::SetGraphDescriptor). END" << endl;
		   return;
	   }

	   closest_vertex_to_origin = topological_map_->GetVertexById(0);
	   closest_vertex_to_destination = closest_vertex_to_origin;

	   double closest_distance_to_origin = sqrt( pow(origin.px - closest_vertex_to_origin->x_coordinate, 2) +
			   pow(origin.py - closest_vertex_to_origin->y_coordinate, 2)  );
	   double closest_distance_to_destination = sqrt( pow(destination.px - closest_vertex_to_destination->x_coordinate, 2) +
			   pow(destination.py - closest_vertex_to_destination->y_coordinate, 2)  );

	   for(int i=0; i < topological_map_->size(); i++){
		   Vertex* vertex = topological_map_->GetVertexById(i);
	   	   double distance_to_origin = sqrt( pow(origin.px - vertex->x_coordinate, 2) +
	   			   pow(origin.py - vertex->y_coordinate, 2)  );
	   	   if(distance_to_origin < closest_distance_to_origin){
	   		   closest_vertex_to_origin = vertex;
	   		   closest_distance_to_origin = distance_to_origin;
	   	   }
	   	   double distance_to_destination = sqrt( pow(destination.px - vertex->x_coordinate, 2) +
	   			   			   pow(destination.py - vertex->y_coordinate, 2)  );
	   	   if(distance_to_destination < closest_distance_to_destination){
	   		   closest_vertex_to_destination = vertex;
	   		   closest_distance_to_destination = distance_to_destination;
	   	   }
	   }
	   cout << "PlannerMap::GetClosestVertices ok - END" << endl;
}


AdjNodeList* PlannerMap::GetAdjacents(int vertex_id){
	   cout << "CALL PlannerMap::GetAdjacents -" << endl;

	   AdjNodeList *list_of_adjacents = topological_map_->GetVertexById(vertex_id)->list_of_adjacents;

	   cout << "PlannerMap::GetAdjacents ok - END" << endl;
	   return list_of_adjacents;
}

Vertex* PlannerMap::GetVertex(int vertex_id){
	   cout << "CALL PlannerMap::GetVertex -" << endl;

	   Vertex *vertex = topological_map_->GetVertexById(vertex_id);

	   cout << "PlannerMap::GetVertex ok - END" << endl;
	   return vertex;
}

int PlannerMap::GetEdgeCost(int vertex_1_id, int vertex_2_id, int *edge_cost){
	   cout << "CALL PlannerMap::GetEdgeCost -" << endl;

	   AdjNode *adj_node = topological_map_->GetVertexById(vertex_1_id)->list_of_adjacents->GetFirst();
	   while(adj_node){
		   if(adj_node->adjacent_id == vertex_2_id){
			   *edge_cost = adj_node->dist;
			   cout << "PlannerMap::GetEdge. valid edge. ok - END" << endl;
			   return kEdgeOk;
		   }
	   }
	   edge_cost = NULL;

	   cout << "PlannerMap::GetEdge. not valid edge. ok - END" << endl;
	   return kInvalidEdge;
}

int PlannerMap::Size(){
	return topological_map_->size();
}

void PlannerMap::Reset(){
	    for (int i = 0; i < topological_map_->size(); i++){
	    	Vertex* vertex = topological_map_->GetVertexById(i);
	    	vertex->parent = NULL;
	    	vertex->f = (-1)*INFINITY;
	    	vertex->g = (-1)*INFINITY;
	    	vertex->h = (-1)*INFINITY;
	    }
}

